/**
 * Implement the predicates for primitive intersections. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file predicates.h
 */

#ifndef __PREDICATE_H
#define __PREDICATE_H

#include <boost/bind.hpp>
#include <boost/lambda/lambda.hpp>

namespace math 
{
extern const double tiny_value ;
extern const double sq_tiny_value ;

/* function prototype */

template <typename value_type, unsigned dim>
	int tri_tri_overlap_test_3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
							const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2);

template <typename value_type, unsigned dim>
int coplanar_tri_tri3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
					   const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2,
					   const vector<value_type, dim> & normal_1, const vector<value_type, dim> & normal_2);

/*
template <typename value_type, unsigned dim>
int tri_tri_intersection_test_3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
								 const vector<value_type, dim> & p2, const
 vector<value_type, dim> & q2, const vector<value_type, dim> & r2,
								 int * coplanar, vector<value_type, dim> & source, vector<value_type, dim> & target);
*/

template <typename value_type, unsigned dim>
	int tri_tri_intersection_test_3d(const Polygon<value_type, dim> pol1, const Polygon<value_type, dim> pol2 ,
								 int * coplanar, vector<value_type, dim> & source, vector<value_type, dim> & target);

template <typename value_type, unsigned dim>
int tri_tri_overlap_test_2d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
							const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2);

template <typename value_type, unsigned dim>
	bool is_point_on_polygon (const Polygon <value_type, dim> & _pol, const vector<value_type, dim> & _p); 



/**
 * Decide the sign of det of 3 x 3. the vector are view as column vector. 
 * This is not robust if the computation is not exact.
 */
template <typename value_type, unsigned dim>
int sign_det_3 (const vector<value_type, dim> & p1, const vector<value_type, dim> & p2,  const vector<value_type, dim> & p3)
{
	value_type v =  ((p1[0]-p3[0])*(p2[1]-p3[1])-(p1[1]-p3[1])*(p2[0]-p3[0]));	
	if (v > value_type(0)) return 1  ; 
	if (v == value_type(0)) return 0 ; 
	if (v < value_type(0)) return -1 ;
}

/*
int tri_tri_overlap_test_3d(double p1[3], double q1[3], double r1[3], 
			    double p2[3], double q2[3], double r2[3]);
*/
/*
int coplanar_tri_tri3d(double  p1[3], double  q1[3], double  r1[3],
		       double  p2[3], double  q2[3], double  r2[3],
		       double  N1[3], double  N2[3]);
*/



/*
int tri_tri_overlap_test_2d(double p1[2], double q1[2], double r1[2], 
			    double p2[2], double q2[2], double r2[2]);
*/
/*
int tri_tri_intersection_test_3d(double p1[3], double q1[3], double r1[3], 
				 double p2[3], double q2[3], double r2[3],
				 int * coplanar, 
				 double source[3],double target[3]);
*/

/* coplanar returns whether the triangles are coplanar  
*  source and target are the endpoints of the segment of 
*  intersection if it exists) 
*/


/* some 3D macros */
/*
#define CROSS(dest,v1,v2)                       \
               dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
               dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
               dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
 


#define SUB(dest,v1,v2) dest[0]=v1[0]-v2[0]; \
                        dest[1]=v1[1]-v2[1]; \
                        dest[2]=v1[2]-v2[2]; 


#define SCALAR(dest,alpha,v) dest[0] = alpha * v[0]; \
                             dest[1] = alpha * v[1]; \
                             dest[2] = alpha * v[2];
*/

/*
#define CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2) {\
  SUB(v1,p2,q1)\
  SUB(v2,p1,q1)\
  CROSS(N1,v1,v2)\
  SUB(v1,q2,q1)\
  if (DOT(v1,N1) > 0.0f) return 0;\
  SUB(v1,p2,p1)\
  SUB(v2,r1,p1)\
  CROSS(N1,v1,v2)\
  SUB(v1,r2,p1) \
  if (DOT(v1,N1) > 0.0f) return 0;\
  else return 1; }
*/
#define CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2) {\
		v1 = p2 - q1;					  \
		v2 = p1 - q1;					  \
		N1 = v1 ^ v2;					  \
		v1 = q2 - q1;							\
		if ((v1*N1) > value_type(0.0)) return 0;\
		v1 = p2 - p1;					   \
		v2 = r1 - p1;					   \
        N1 = v1 ^ v2;					   \
        v1 = r2 - p1;					   \
        if ((v1*N1) > 0.0f) return 0;\
        else return 1; }


/* Permutation in a canonical form of T2's vertices */

#define TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2) { \
  if (dp2 > 0.0f) { \
     if (dq2 > 0.0f) CHECK_MIN_MAX(p1,r1,q1,r2,p2,q2) \
     else if (dr2 > 0.0f) CHECK_MIN_MAX(p1,r1,q1,q2,r2,p2)\
     else CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2) }\
  else if (dp2 < 0.0f) { \
    if (dq2 < 0.0f) CHECK_MIN_MAX(p1,q1,r1,r2,p2,q2)\
    else if (dr2 < 0.0f) CHECK_MIN_MAX(p1,q1,r1,q2,r2,p2)\
    else CHECK_MIN_MAX(p1,r1,q1,p2,q2,r2)\
  } else { \
    if (dq2 < 0.0f) { \
      if (dr2 >= 0.0f)  CHECK_MIN_MAX(p1,r1,q1,q2,r2,p2)\
      else CHECK_MIN_MAX(p1,q1,r1,p2,q2,r2)\
    } \
    else if (dq2 > 0.0f) { \
      if (dr2 > 0.0f) CHECK_MIN_MAX(p1,r1,q1,p2,q2,r2)\
      else  CHECK_MIN_MAX(p1,q1,r1,q2,r2,p2)\
    } \
    else  { \
      if (dr2 > 0.0f) CHECK_MIN_MAX(p1,q1,r1,r2,p2,q2)\
      else if (dr2 < 0.0f) CHECK_MIN_MAX(p1,r1,q1,r2,p2,q2)\
      else return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);\
     }}}
  


/*
*
*  Three-dimensional Triangle-Triangle Overlap Test
*
*/
template <typename value_type, unsigned dim>
int tri_tri_overlap_test_3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
							const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2)

/*int tri_tri_overlap_test_3d(double p1[3], double q1[3], double r1[3], 

  double p2[3], double q2[3], double r2[3])*/
{
	//double dp1, dq1, dr1, dp2, dq2, dr2;
	//double v1[3], v2[3];
	//double N1[3], N2[3]; 
    value_type dp1, dq1, dr1, dp2, dq2, dr2;
	vector<value_type, dim> v1(3), v2(3);
	vector<value_type, dim> N1(3), N2(3); 
  
  
	/* Compute distance signs  of p1, q1 and r1 to the plane of
	   triangle(p2,q2,r2) */

	v1  =  p2 - r2  ;//SUB(v1,p2,r2)
	v2  =  q2 - r2  ;//SUB(v2,q2,r2)
	N2  =  v1 ^ v2  ;//CROSS(N2,v1,v2)

	v1  =  p1 - r2  ;//SUB(v1,p1,r2)
	dp1 =  v1 * N2  ;//dp1 = DOT(v1,N2);
	v1  =  q1 - r2  ;//SUB(v1,q1,r2)
	dq1 =  v1 * N2  ;//dq1 = DOT(v1,N2);
	v1  =  r1 - r2  ;//SUB(v1,r1,r2)
	dr1 =  v1 * N2  ;//dr1 = DOT(v1,N2);
  
  
	if (((dp1 * dq1) > 0.0f) && ((dp1 * dr1) > 0.0f))  return 0; 

	/* Compute distance signs  of p2, q2 and r2 to the plane of
	   triangle(p1,q1,r1) */

	v1  =  q1 - p1  ;//SUB(v1,q1,p1)
	v2  =  r1 - p1  ;//SUB(v2,r1,p1)
	N1  =  v1 ^ v2  ;//CROSS(N1,v1,v2)

	v1  =  p2 - r1  ;//SUB(v1,p2,r1)
	dp2 =  v1 * N1  ;//dp2 = DOT(v1,N1);
	v1  =  q2 - r1  ;//SUB(v1,q2,r1)
	dq2 =  v1 * N1  ;//dq2 = DOT(v1,N1);
	v1  =  r2 - r1  ;//SUB(v1,r2,r1)
	dr2 =  v1 * N1  ;//dr2 = DOT(v1,N1);
  
	if (((dp2 * dq2) > 0.0f) && ((dp2 * dr2) > 0.0f)) return 0;

	/* Permutation in a canonical form of T1's vertices */


	if (dp1 > 0.0f) {
		if (dq1 > 0.0f) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
			else if (dr1 > 0.0f) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)	
				else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
						 } else if (dp1 < 0.0f) {
		if (dq1 < 0.0f) TRI_TRI_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
			else if (dr1 < 0.0f) TRI_TRI_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
				else TRI_TRI_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
						 } else {
		if (dq1 < 0.0f) {
			if (dr1 >= 0.0f) TRI_TRI_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
				else TRI_TRI_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
						 }
		else if (dq1 > 0.0f) {
			if (dr1 > 0.0f) TRI_TRI_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
				else TRI_TRI_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
						 }
		else  {
			if (dr1 > 0.0f) TRI_TRI_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
				else if (dr1 < 0.0f) TRI_TRI_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
					else return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);
		}
	}
};


template <typename value_type, unsigned dim>
int coplanar_tri_tri3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
					   const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2,
					   const vector<value_type, dim> & normal_1, const vector<value_type, dim> & normal_2)
/*
	double p1[3], double q1[3], double r1[3],
		       double p2[3], double q2[3], double r2[3],
		       double normal_1[3], double normal_2[3])
*/
/*
int coplanar_tri_tri3d(double p1[3], double q1[3], double r1[3],
		       double p2[3], double q2[3], double r2[3],
		       double normal_1[3], double normal_2[3])
*/
{
	/*
  double P1[2],Q1[2],R1[2];
  double P2[2],Q2[2],R2[2];

  double n_x, n_y, n_z;
	*/

	vector<value_type, dim> P1(2),Q1(2),R1(2);
    vector<value_type, dim> P2(2),Q2(2),R2(2);
  value_type n_x, n_y, n_z;


  n_x = ((normal_1[0]<0)?-normal_1[0]:normal_1[0]);
  n_y = ((normal_1[1]<0)?-normal_1[1]:normal_1[1]);
  n_z = ((normal_1[2]<0)?-normal_1[2]:normal_1[2]);


  /* Projection of the triangles in 3D onto 2D such that the area of
     the projection is maximized. */


  if (( n_x > n_z ) && ( n_x >= n_y )) {
    // Project onto plane YZ

      P1[0] = q1[2]; P1[1] = q1[1];
      Q1[0] = p1[2]; Q1[1] = p1[1];
      R1[0] = r1[2]; R1[1] = r1[1]; 
    
      P2[0] = q2[2]; P2[1] = q2[1];
      Q2[0] = p2[2]; Q2[1] = p2[1];
      R2[0] = r2[2]; R2[1] = r2[1]; 

  } else if (( n_y > n_z ) && ( n_y >= n_x )) {
    // Project onto plane XZ

    P1[0] = q1[0]; P1[1] = q1[2];
    Q1[0] = p1[0]; Q1[1] = p1[2];
    R1[0] = r1[0]; R1[1] = r1[2]; 
 
    P2[0] = q2[0]; P2[1] = q2[2];
    Q2[0] = p2[0]; Q2[1] = p2[2];
    R2[0] = r2[0]; R2[1] = r2[2]; 
    
  } else {
    // Project onto plane XY

    P1[0] = p1[0]; P1[1] = p1[1]; 
    Q1[0] = q1[0]; Q1[1] = q1[1]; 
    R1[0] = r1[0]; R1[1] = r1[1]; 
    
    P2[0] = p2[0]; P2[1] = p2[1]; 
    Q2[0] = q2[0]; Q2[1] = q2[1]; 
    R2[0] = r2[0]; R2[1] = r2[1]; 
  }

  return tri_tri_overlap_test_2d(P1,Q1,R1,P2,Q2,R2);
    
};



/*
*                                                                
*  Three-dimensional Triangle-Triangle Intersection              
*
*/

/*
   This macro is called when the triangles surely intersect
   It constructs the segment of intersection of the two triangles
   if they are not coplanar.
*/

#define CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2) { \
		v1 = q1 - p1;\
		v2 = r2 - p1;\
        N  = v1 ^ v2;\
		v  = p2 - p1;\
		if ((v*N) > value_type(0.0)) {\
		v1 = r1 - p1;\
		N  = v1 ^ v2;\
		if ((v*N) <= value_type(0.0)) {\
		v2 = q2 - p1;\
		N  = v1 ^ v2;\
		if ((v*N) > value_type(0.0)) {\
		v1  = p1 - p2;\
		v2  = p1 - r1;\
		alpha = (v1*N2) / (v2*N2);\
		v1  = v2 * alpha ;\
		source = p1 - v1 ;\
		v1  = p2 - p1;\
		v2  = p2 - r2;\
		alpha = (v1*N1) / (v2*N1);\
		v1  = v2 * alpha;\
	    target = p2 - v1 ;\
		return 1;		  \
      } else { \
		v1  = p2 - p1;\
		v2  = p2 - q2;\
		alpha = (v1*N1) / (v2*N1);\
		v1  = v2 * alpha;\
		source = p2 - v1;\
		v1  = p2 - p1;\
		v2  = p2 - r2;\
		alpha = (v1*N1) / (v2*N1); \
		v1  = v2 * alpha;\
		target = p2 - v1;\
	return 1; \
      } \
    } else { \
      return 0; \
    } \
  } else { \
		v2 = q2 - p1;\
		N  = v1 ^ v2;\
		if ((v*N) < value_type(0.0)) {\
      return 0; \
    } else { \
		v1 = r1 - p1;\
		N  = v1 ^ v2;\
		if ((v*N) >= value_type(0.0)) {\
		v1 = p1 - p2;\
		v2 = p1 - r1;\
		alpha = (v1*N2) / (v2*N2);\
		v1 = v2 * alpha;\
		source = p1 - v1;\
		v1 = p1 - p2;\
		v2 = p1 - q1;\
		alpha = (v1*N2) / (v2*N2);\
		v1 = v2 * alpha;\
		target = p1 - v1;\
	return 1; \
      } else { \
		v1 = p2 - p1;\
		v2 = p2 - q2;\
		alpha = (v1*N1) / (v2*N1);\
		v1 = v2 * alpha;\
		source = p2 - v1;\
		v1 = p1 - p2;\
		v2 = p1 - q1;\
		alpha = (v1*N2) / (v2*N2); \
		v1 = v2 * alpha;\
		target = p1 - v1;\
	return 1; \
      }}}} 
/*
#define CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2) { \
  SUB(v1,q1,p1) \
  SUB(v2,r2,p1) \
  CROSS(N,v1,v2) \
  SUB(v,p2,p1) \
  if (DOT(v,N) > 0.0f) {\
    SUB(v1,r1,p1) \
    CROSS(N,v1,v2) \
    if (DOT(v,N) <= 0.0f) { \
      SUB(v2,q2,p1) \
      CROSS(N,v1,v2) \
      if (DOT(v,N) > 0.0f) { \
	SUB(v1,p1,p2) \
	SUB(v2,p1,r1) \
	alpha = DOT(v1,N2) / DOT(v2,N2); \
	SCALAR(v1,alpha,v2) \
	SUB(source,p1,v1) \
	SUB(v1,p2,p1) \
	SUB(v2,p2,r2) \
	alpha = DOT(v1,N1) / DOT(v2,N1); \
	SCALAR(v1,alpha,v2) \
	SUB(target,p2,v1) \
	return 1; \
      } else { \
	SUB(v1,p2,p1) \
	SUB(v2,p2,q2) \
	alpha = DOT(v1,N1) / DOT(v2,N1); \
	SCALAR(v1,alpha,v2) \
	SUB(source,p2,v1) \
	SUB(v1,p2,p1) \
	SUB(v2,p2,r2) \
	alpha = DOT(v1,N1) / DOT(v2,N1); \
	SCALAR(v1,alpha,v2) \
	SUB(target,p2,v1) \
	return 1; \
      } \
    } else { \
      return 0; \
    } \
  } else { \
    SUB(v2,q2,p1) \
    CROSS(N,v1,v2) \
    if (DOT(v,N) < 0.0f) { \
      return 0; \
    } else { \
      SUB(v1,r1,p1) \
      CROSS(N,v1,v2) \
      if (DOT(v,N) >= 0.0f) { \
	SUB(v1,p1,p2) \
	SUB(v2,p1,r1) \
	alpha = DOT(v1,N2) / DOT(v2,N2); \
	SCALAR(v1,alpha,v2) \
	SUB(source,p1,v1) \
	SUB(v1,p1,p2) \
	SUB(v2,p1,q1) \
	alpha = DOT(v1,N2) / DOT(v2,N2); \
	SCALAR(v1,alpha,v2) \
	SUB(target,p1,v1) \
	return 1; \
      } else { \
	SUB(v1,p2,p1) \
	SUB(v2,p2,q2) \
	alpha = DOT(v1,N1) / DOT(v2,N1); \
	SCALAR(v1,alpha,v2) \
	SUB(source,p2,v1) \
	SUB(v1,p1,p2) \
	SUB(v2,p1,q1) \
	alpha = DOT(v1,N2) / DOT(v2,N2); \
	SCALAR(v1,alpha,v2) \
	SUB(target,p1,v1) \
	return 1; \
      }}}} 
*/
								

#define TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2) { \
  if (dp2 > 0.0f) { \
     if (dq2 > 0.0f) CONSTRUCT_INTERSECTION(p1,r1,q1,r2,p2,q2) \
     else if (dr2 > 0.0f) CONSTRUCT_INTERSECTION(p1,r1,q1,q2,r2,p2)\
     else CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2) }\
  else if (dp2 < 0.0f) { \
    if (dq2 < 0.0f) CONSTRUCT_INTERSECTION(p1,q1,r1,r2,p2,q2)\
    else if (dr2 < 0.0f) CONSTRUCT_INTERSECTION(p1,q1,r1,q2,r2,p2)\
    else CONSTRUCT_INTERSECTION(p1,r1,q1,p2,q2,r2)\
  } else { \
    if (dq2 < 0.0f) { \
      if (dr2 >= 0.0f)  CONSTRUCT_INTERSECTION(p1,r1,q1,q2,r2,p2)\
      else CONSTRUCT_INTERSECTION(p1,q1,r1,p2,q2,r2)\
    } \
    else if (dq2 > 0.0f) { \
      if (dr2 > 0.0f) CONSTRUCT_INTERSECTION(p1,r1,q1,p2,q2,r2)\
      else  CONSTRUCT_INTERSECTION(p1,q1,r1,q2,r2,p2)\
    } \
    else  { \
      if (dr2 > 0.0f) CONSTRUCT_INTERSECTION(p1,q1,r1,r2,p2,q2)\
      else if (dr2 < 0.0f) CONSTRUCT_INTERSECTION(p1,r1,q1,r2,p2,q2)\
      else { \
       	*coplanar = 1; \
	return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);\
     } \
  }} }
  

/*
   The following version computes the segment of intersection of the
   two triangles if it exists. 
   coplanar returns whether the triangles are coplanar
   source and target are the endpoints of the line segment of intersection 
   @return 
   - 0: Non-intersect
   - 1: segment
   - 2: point
   - 3: polygon
   
*/
/*
template <typename value_type, unsigned dim>
int tri_tri_intersection_test_3d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
								 const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2,
								 int * coplanar, vector<value_type, dim> & source, vector<value_type, dim> & target)
*/
/*
int tri_tri_intersection_test_3d(double p1[3], double q1[3], double r1[3], 
				 double p2[3], double q2[3], double r2[3],
				 int * coplanar, 
				 double source[3], double target[3] )*/

template <typename value_type, unsigned dim>
	int tri_tri_intersection_test_3d(const Polygon<value_type, dim> pol1, const Polygon<value_type, dim> pol2 ,
								 int * coplanar, vector<value_type, dim> & source, vector<value_type, dim> & target)	 
{
	/*
	  double dp1, dq1, dr1, dp2, dq2, dr2;
	  double v1[3], v2[3], v[3];
	  double N1[3], N2[3], N[3];
	  double alpha;*/

	const vector<value_type, dim> & p1 = pol1[0];
	const vector<value_type, dim> & q1 = pol1[1];
	const vector<value_type, dim> & r1 = pol1[2];

	const vector<value_type, dim> & p2 = pol2[0];
	const vector<value_type, dim> & q2 = pol2[1];
	const vector<value_type, dim> & r2 = pol2[2];


	value_type dp1, dq1, dr1, dp2, dq2, dr2;
	vector<value_type, dim>  v1, v2, v(3) ;  //vector<value_type, dim> v1(3), v2(3), v(3);
	vector<value_type, dim>  N1, N2, N(3) ;  //vector<value_type, dim> N1(3), N2(3), N(3);
	value_type alpha;

	// Compute distance signs  of p1, q1 and r1 
	// to the plane of triangle(p2,q2,r2)


	v1  =  p2 - r2  ;//SUB(v1,p2,r2)
	v2  =  q2 - r2  ;//SUB(v2,q2,r2)
	N2  =  v1 ^ v2  ;//CROSS(N2,v1,v2)

	v1  =  p1 - r2  ;//SUB(v1,p1,r2)
	dp1 =  v1 * N2  ;//dp1 = DOT(v1,N2);
	v1  =  q1 - r2  ;//SUB(v1,q1,r2)
	dq1 =  v1 * N2  ;//dq1 = DOT(v1,N2);
	v1  =  r1 - r2  ;//SUB(v1,r1,r2)
	dr1 =  v1 * N2  ;//dr1 = DOT(v1,N2);
  
	if (((dp1 * dq1) > value_type(0)) && ((dp1 * dr1) > value_type(0)))  return 0; 

	// Only one point is on the plane  
	if (dp1 == value_type(0) && dq1 * dr1 > value_type(0))
	{
		//std::cout<<"\n P1 is on plane";
		if (is_point_on_polygon(pol2, p1)) 
		{
			//std::cout<<"\n P1 is the intersection point. ";
			source = p1 ;
			return 2 ;
		}
	}
	else if (dq1 == value_type(0) && dp1 * dr1 > value_type(0))
	{
		//std::cout<<"\n Q1 is on plane";
		if (is_point_on_polygon(pol2, q1)) 
		{
			//std::cout<<"\n Q1 is the intersection point. ";
			source = q1 ;
			return 2 ;
		}
	}
	else if (dr1 == value_type(0) && dp1 * dq1 > value_type(0))
	{
		//std::cout<<"\n R1 is on plane";
		if (is_point_on_polygon(pol2, r1)) 
		{
			//std::cout<<"\n R1 is the intersection point. ";
			source = r1 ;
			return 2 ;
		}
	}


	// Compute distance signs  of p2, q2 and r2 
	// to the plane of triangle(p1,q1,r1)

  
	v1  =  q1 - p1  ;//SUB(v1,q1,p1)
	v2  =  r1 - p1  ;//SUB(v2,r1,p1)
	N1  =  v1 ^ v2  ;//CROSS(N1,v1,v2)

	v1  =  p2 - r1  ;//SUB(v1,p2,r1)
	dp2 =  v1 * N1  ;//dp2 = DOT(v1,N1);
	v1  =  q2 - r1  ;//SUB(v1,q2,r1)
	dq2 =  v1 * N1  ;//dq2 = DOT(v1,N1);
	v1  =  r2 - r1  ;//SUB(v1,r2,r1)
	dr2 =  v1 * N1  ;//dr2 = DOT(v1,N1);
  
	if (((dp2 * dq2) > 0.0f) && ((dp2 * dr2) > value_type(0.0))) return 0;
	// Only one point is on the plane

	if (dp2 == value_type(0) && dq2 * dr2 > value_type(0))
	{
		//std::cout<<"\n P2 is on plane";
		if (is_point_on_polygon(pol1, p2)) 
		{
			//std::cout<<"\n P2 is the intersection point. ";
			source = p2 ;
			return 2 ;
		}
	}
	else if (dq2 == value_type(0) && dp2 * dr2 > value_type(0))
	{
		//std::cout<<"\n Q2 is on plane";
		if (is_point_on_polygon(pol1, q2)) 
		{
			//std::cout<<"\n Q2 is the intersection point. ";
			source = q2 ;
			return 2 ;
		}
	}
	else if (dr2 == value_type(0) && dp2 * dq2 > value_type(0))
	{
		//std::cout<<"\n R2 is on plane";
		if (is_point_on_polygon(pol1, r2)) 
		{
			//std::cout<<"\n R2 is the intersection point. ";
			source = r2 ;
			return 2 ;
		}
	}


	// Permutation in a canonical form of T1's vertices

	if (dp1 > 0.0f) {
		if (dq1 > 0.0f) TRI_TRI_INTER_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
			else if (dr1 > 0.0f) TRI_TRI_INTER_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
	
				else TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
						 } else if (dp1 < 0.0f) {
		if (dq1 < 0.0f) TRI_TRI_INTER_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
			else if (dr1 < 0.0f) TRI_TRI_INTER_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
				else TRI_TRI_INTER_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
						 } else {
		if (dq1 < 0.0f) {
			if (dr1 >= 0.0f) TRI_TRI_INTER_3D(q1,r1,p1,p2,r2,q2,dp2,dr2,dq2)
				else TRI_TRI_INTER_3D(p1,q1,r1,p2,q2,r2,dp2,dq2,dr2)
						 }
		else if (dq1 > 0.0f) {
			if (dr1 > 0.0f) TRI_TRI_INTER_3D(p1,q1,r1,p2,r2,q2,dp2,dr2,dq2)
				else TRI_TRI_INTER_3D(q1,r1,p1,p2,q2,r2,dp2,dq2,dr2)
						 }
		else  {
			if (dr1 > 0.0f) TRI_TRI_INTER_3D(r1,p1,q1,p2,q2,r2,dp2,dq2,dr2)
				else if (dr1 < 0.0f) TRI_TRI_INTER_3D(r1,p1,q1,p2,r2,q2,dp2,dr2,dq2)
					else {
						// triangles are co-planar

						*coplanar = 1;
						return coplanar_tri_tri3d(p1,q1,r1,p2,q2,r2,N1,N2);
					}
		}
	}
};





/*
*
*  Two dimensional Triangle-Triangle Overlap Test    
*
*/


/* some 2D macros */

#define ORIENT_2D(a, b, c)  ((a[0]-c[0])*(b[1]-c[1])-(a[1]-c[1])*(b[0]-c[0]))


#define INTERSECTION_TEST_VERTEX(P1, Q1, R1, P2, Q2, R2) {\
  if (ORIENT_2D(R2,P2,Q1) >= 0.0f)\
    if (ORIENT_2D(R2,Q2,Q1) <= 0.0f)\
      if (ORIENT_2D(P1,P2,Q1) > 0.0f) {\
	if (ORIENT_2D(P1,Q2,Q1) <= 0.0f) return 1; \
	else return 0;} else {\
	if (ORIENT_2D(P1,P2,R1) >= 0.0f)\
	  if (ORIENT_2D(Q1,R1,P2) >= 0.0f) return 1; \
	  else return 0;\
	else return 0;}\
    else \
      if (ORIENT_2D(P1,Q2,Q1) <= 0.0f)\
	if (ORIENT_2D(R2,Q2,R1) <= 0.0f)\
	  if (ORIENT_2D(Q1,R1,Q2) >= 0.0f) return 1; \
	  else return 0;\
	else return 0;\
      else return 0;\
  else\
    if (ORIENT_2D(R2,P2,R1) >= 0.0f) \
      if (ORIENT_2D(Q1,R1,R2) >= 0.0f)\
	if (ORIENT_2D(P1,P2,R1) >= 0.0f) return 1;\
	else return 0;\
      else \
	if (ORIENT_2D(Q1,R1,Q2) >= 0.0f) {\
	  if (ORIENT_2D(R2,R1,Q2) >= 0.0f) return 1; \
	  else return 0; }\
	else return 0; \
    else  return 0; \
 };



#define INTERSECTION_TEST_EDGE(P1, Q1, R1, P2, Q2, R2) { \
  if (ORIENT_2D(R2,P2,Q1) >= 0.0f) {\
    if (ORIENT_2D(P1,P2,Q1) >= 0.0f) { \
        if (ORIENT_2D(P1,Q1,R2) >= 0.0f) return 1; \
        else return 0;} else { \
      if (ORIENT_2D(Q1,R1,P2) >= 0.0f){ \
	if (ORIENT_2D(R1,P1,P2) >= 0.0f) return 1; else return 0;} \
      else return 0; } \
  } else {\
    if (ORIENT_2D(R2,P2,R1) >= 0.0f) {\
      if (ORIENT_2D(P1,P2,R1) >= 0.0f) {\
	if (ORIENT_2D(P1,R1,R2) >= 0.0f) return 1;  \
	else {\
	  if (ORIENT_2D(Q1,R1,R2) >= 0.0f) return 1; else return 0;}}\
      else  return 0; }\
    else return 0; }}


template <typename value_type, unsigned dim>
int ccw_tri_tri_intersection_2d (const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2)
/*
int ccw_tri_tri_intersection_2d(double p1[2], double q1[2], double r1[2], 
				double p2[2], double q2[2], double r2[2]) 
*/
{
  if ( ORIENT_2D(p2,q2,p1) >= 0.0f ) {
    if ( ORIENT_2D(q2,r2,p1) >= 0.0f ) {
      if ( ORIENT_2D(r2,p2,p1) >= 0.0f ) return 1;
      else INTERSECTION_TEST_EDGE(p1,q1,r1,p2,q2,r2)
    } else {  
      if ( ORIENT_2D(r2,p2,p1) >= 0.0f ) 
	INTERSECTION_TEST_EDGE(p1,q1,r1,r2,p2,q2)
      else INTERSECTION_TEST_VERTEX(p1,q1,r1,p2,q2,r2)}}
  else {
    if ( ORIENT_2D(q2,r2,p1) >= 0.0f ) {
      if ( ORIENT_2D(r2,p2,p1) >= 0.0f ) 
	INTERSECTION_TEST_EDGE(p1,q1,r1,q2,r2,p2)
      else  INTERSECTION_TEST_VERTEX(p1,q1,r1,q2,r2,p2)}
    else INTERSECTION_TEST_VERTEX(p1,q1,r1,r2,p2,q2)}
};


template<typename value_type, unsigned dim>
int tri_tri_overlap_test_2d(const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1, 
							const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2)
/*
int tri_tri_overlap_test_2d(double p1[2], double q1[2], double r1[2], 
			    double p2[2], double q2[2], double r2[2]) 
*/
{
  if ( ORIENT_2D(p1,q1,r1) < 0.0f )
    if ( ORIENT_2D(p2,q2,r2) < 0.0f )
      return ccw_tri_tri_intersection_2d(p1,r1,q1,p2,r2,q2);
    else
      return ccw_tri_tri_intersection_2d(p1,r1,q1,p2,q2,r2);
  else
    if ( ORIENT_2D(p2,q2,r2) < 0.0f )
      return ccw_tri_tri_intersection_2d(p1,q1,r1,p2,r2,q2);
    else
      return ccw_tri_tri_intersection_2d(p1,q1,r1,p2,q2,r2);

};

/**
 * We compute the intersection of two triangles on 2D. 
 - If the polygon size is 1. The intersection part is a point.
 - If the polygon size is 2. The intersection part is a segment.
 - If the polygon isze is >=3. The intersection part is a polygon. 
 */
template<typename value_type, unsigned dim>
int tri_tri_intersection_test_2d (const vector<value_type, dim> & p1, const vector<value_type, dim> & q1, const vector<value_type, dim> & r1,
								  const vector<value_type, dim> & p2, const vector<value_type, dim> & q2, const vector<value_type, dim> & r2, Polygon<value_type, dim> & pol)
{
	return false; 
}

template <typename value_type>
bool polygon_sphere_test(const Polygon<value_type, 2> & pol, const Sphere <value_type, 2> & sphere)
{
	unsigned size = pol.size(); 
	for (unsigned i = 0; i < size; ++i)
	{
		const vector <value_type, 2> & p1 = pol[i] ;
		const vector <value_type, 2> & p2 = pol[(i + 1)% size] ;
		if (sphere.is_point_inside(p1) ) return true; 
		if (sphere.is_collide_with_segment (Segment<value_type, 2> (p1, p2))) return true; 
	}
	return false; 
}

/**
 * Is point in polygon
 */
template <typename value_type, unsigned dim>
	bool is_point_on_polygon (const Polygon <value_type, dim> & _pol, const vector<value_type, dim> & _p)
{
	unsigned size = _pol.size();
	// Check if the point is on the vertex .
	for (unsigned i = 0; i < size; ++ i)
	{
		if (_pol[i] == _p  )
		//if ((_pol[i] - _p ).square_module() < TINY)
		{
			return true; 
		}
	}
	// Map the polygon and point to the 2D domain. 
	unsigned x, y;
	vector<value_type, dim> normal = _pol.calculate_normal(); 
	normal.normalize();

//		value_typeType area = value_typeType(0);
//		value_typeType an = normal.module();
	value_type ax, ay, az ;
	unsigned coord ; // coord to be ignored. 1-x, 2-y, 3-z
		
	ax = abs(normal[0]) ;
	ay = abs(normal[1]) ;
	az = abs(normal[2]) ;		
	coord = 2; 
	if (ax > ay )
	{
		if (ax > az) coord = 0;
	}
	else
	{
		if (ay > az) coord = 1 ;
	}

	switch (coord)
	{
	case 0:
		x = 1; y = 2; 
		break; 
	case 1:
		x = 2; y = 0; 
		break; 
	case 2:
		x = 0; y = 1;
		break; 
	}
		
	bool rvalue = false; 
	for (unsigned i = 0, j = size - 1; i < size; j = i ++)
	{
		if ((((_pol[i][y] <= _p[y]) && (_p[y] < _pol[j][y])) ||
			 ((_pol[j][y] <= _p[y]) && (_p[y] < _pol[i][y]))) &&
			(_p[x] < (_pol[j][x] - _pol[i][x]) * (_p[y] - _pol[i][y]) / ( _pol[j][y] -  _pol[i][y]) + _pol[i][x]))
			rvalue = !  rvalue;  					
	}
	return rvalue; 
}
}
#endif
